//
// Created by Michel Lesoinne on 6/23/22.
//
#include <hardware/i2c.h>

#include "SSD1306.h"

namespace pico_ssd1306 {
SSD1306::SSD1306(i2c_inst *i2CInst, uint16_t Address, Size size)
    : dmaChannel(
          {
              .transferSize = DMA_SIZE_16,
              .dreq = i2c_get_dreq(i2CInst, false), //
              .read_addr = nullptr,  // Not the right one, we'll never read.
          },
          {.transferSize = DMA_SIZE_16,
           .dreq = i2c_get_dreq(i2CInst, true), //
           .write_addr = &i2CInst->hw->data_cmd})
{
	// Set class instanced variables
	this->i2CInst = i2CInst;
	this->address = Address;
	this->size = size;

	this->width = 128;

	if (size == Size::W128xH32) {
		this->height = 32;
	} else {
		this->height = 64;
	}

	// create a frame buffer
	this->frameBuffer = FrameBuffer();

	// display is not inverted by default
	this->inverted = false;

	// this is a list of setup commands for the display
	uint8_t setup[] = {
	    SSD1306_DISPLAY_OFF,
	    SSD1306_LOWCOLUMN,
	    SSD1306_HIGHCOLUMN,
	    SSD1306_STARTLINE,

	    SSD1306_MEMORYMODE,
	    SSD1306_MEMORYMODE_HORZONTAL,

	    SSD1306_CONTRAST,
	    0xFF,

	    SSD1306_INVERTED_OFF,

	    SSD1306_MULTIPLEX,
	    63,

	    SSD1306_DISPLAYOFFSET,
	    0x00,

	    SSD1306_DISPLAYCLOCKDIV,
	    0x80,

	    SSD1306_PRECHARGE,
	    0x22,

	    SSD1306_COMPINS,
	    0x12,

	    SSD1306_VCOMDETECT,
	    0x40,

	    SSD1306_CHARGEPUMP,
	    0x14,

	    SSD1306_DISPLAYALL_ON_RESUME,
	    SSD1306_DISPLAY_ON
	};

	// send each one of the setup commands
	for (uint8_t &command: setup) {
		this->cmd(command);
	}

	// clear the buffer and send it to the display
	// if not done display shows garbage data
	this->clear();
	this->blockingSendBuffer();

}

void SSD1306::setPixel(int16_t x, int16_t y, WriteMode mode) {
	// return if position out of bounds
	if ((x < 0) || (x >= this->width) || (y < 0) || (y >= this->height)) return;

	// byte to be used for buffer operation
	uint8_t byte;

	// display with 32 px height requires doubling of set bits, reason to this is explained in readme
	// this shifts 1 to byte based on y coordinate
	// remember that buffer is a one dimension array, so we have to calculate offset from coordinates
	if (size == Size::W128xH32) {
		y = (y << 1) + 1;
		byte = 1 << (y & 7);
		char byte_offset = byte >> 1;
		byte = byte | byte_offset;
	} else {
		byte = 1 << (y & 7);
	}

	// check the write mode and manipulate the frame buffer
	if (mode == WriteMode::ADD) {
		this->frameBuffer.byteOR(x + (y / 8) * this->width, byte);
	} else if (mode == WriteMode::SUBTRACT) {
		this->frameBuffer.byteAND(x + (y / 8) * this->width, ~byte);
	} else if (mode == WriteMode::INVERT) {
		this->frameBuffer.byteXOR(x + (y / 8) * this->width, byte);
	}


}

void SSD1306::blockingSendBuffer() {
	this->cmd(SSD1306_PAGEADDR); //Set page address from min to max
	this->cmd(0x00);
	this->cmd(0x07);
	this->cmd(SSD1306_COLUMNADDR); //Set column address from min to max
	this->cmd(0x00);
	this->cmd(127);

	// create a temporary buffer of size of buffer plus 1 byte for startline command aka 0x40
	unsigned char data[FRAMEBUFFER_SIZE + 1];

	data[0] = SSD1306_STARTLINE;

	// copy framebuffer to temporary buffer
	memcpy(data + 1, frameBuffer.get(), FRAMEBUFFER_SIZE);

	// send data to device
	i2c_write_blocking(this->i2CInst, this->address, data, FRAMEBUFFER_SIZE + 1, false);
}

task<>
SSD1306::sendDataNonBlocking()
{
	i2CInst->hw->enable = 0;
	i2CInst->hw->tar = address;
	i2CInst->hw->enable = 1;

	bool nostop = false;

	auto fb = frameBuffer.get();

	bool first = true;
	bool last = false;
	dmaBuffer[0] = bool_to_bit(i2CInst->restart_on_next) << I2C_IC_DATA_CMD_RESTART_LSB |
	               SSD1306_STARTLINE;
	for (int i = 0; i < FRAMEBUFFER_SIZE; ++i) {
		last = i+1 == FRAMEBUFFER_SIZE;
		dmaBuffer[i + 1] = bool_to_bit(last && !nostop) << I2C_IC_DATA_CMD_STOP_LSB | fb[i];
	}
	// Send via DMA
	co_await dmaChannel.send(std::span<uint16_t>(dmaBuffer));
	// nostop means we are now at the end of a *message* but not the end of a *transfer*
	i2CInst->restart_on_next = nostop;
	co_return;
}


task<>
SSD1306::sendBuffer() {
	this->cmd(SSD1306_PAGEADDR); //Set page address from min to max
	this->cmd(0x00);
	this->cmd(0x07);
	this->cmd(SSD1306_COLUMNADDR); //Set column address from min to max
	this->cmd(0x00);
	this->cmd(127);

	// create a temporary buffer of size of buffer plus 1 byte for startline command aka 0x40
	unsigned char data[FRAMEBUFFER_SIZE + 1];

	data[0] = SSD1306_STARTLINE;

	// copy framebuffer to temporary buffer
	memcpy(data + 1, frameBuffer.get(), FRAMEBUFFER_SIZE);

	// send data to device
	co_await sendDataNonBlocking();
	co_return;
}

void SSD1306::clear() {
	this->frameBuffer.clear();
}

void SSD1306::setOrientation(bool orientation) {
	// remap columns and rows scan direction, effectively flipping the image on display
	if (orientation) {
		this->cmd(SSD1306_CLUMN_REMAP_OFF);
		this->cmd(SSD1306_COM_REMAP_OFF);
	} else {
		this->cmd(SSD1306_CLUMN_REMAP_ON);
		this->cmd(SSD1306_COM_REMAP_ON);
	}
}

void
SSD1306::addBitmapImage(int16_t anchorX, int16_t anchorY, uint8_t image_width, uint8_t image_height,
                        uint8_t *image,
                        WriteMode mode) {
	uint8_t byte;
	// goes over every single bit in image and sets pixel data on its coordinates
	for (uint8_t y = 0; y < image_height; y++) {
		for (uint8_t x = 0; x < image_width / 8; x++) {
			byte = image[y * (image_width / 8) + x];
			for (uint8_t z = 0; z < 8; z++) {
				if ((byte >> (7 - z)) & 1) {
					this->setPixel(x * 8 + z + anchorX, y + anchorY, mode);
				}
			}
		}
	}

}

void SSD1306::invertDisplay() {
	this->cmd(SSD1306_INVERTED_OFF | !this->inverted);
	inverted = !inverted;
}

void SSD1306::cmd(unsigned char command) {
	// 0x00 is a byte indicating to ssd1306 that a command is being sent
	uint8_t data[2] = {0x00, command};
	i2c_write_blocking(this->i2CInst, this->address, data, 2, false);
}


void SSD1306::setContrast(unsigned char contrast) {
	this->cmd(SSD1306_CONTRAST);
	this->cmd(contrast);
}

void SSD1306::setBuffer(unsigned char * buffer) {
	this->frameBuffer.setBuffer(buffer);
}

}